/*
  Program Name:  MC_Main
  Project:       LLH Hopscotch
  Written By:    Daniel Lee
  Revision:      0.6.2
  Change log:   
    0.6.1...    Adding console debugging
        Adding code to send manual commands from the serial console
        for test the various functions of the network.
    0.6.2...    
*/

#include<SoftwareSerial.h>

// Pins 2 and 7 are reserved for FTDI comms
// Pins 0 and 1 are reserved for USB comms (during programming only)

#define TRXRATE 19200
#define TERMBAUD 9600
#define LEDPIN 3
#define SENSORPIN A5
#define SPKRPIN 11
#define SEEDPIN A1
#define RX_boards 19
#define TX_boards 18
#define MAX_BUFFER_SIZE 64
#define MAX_NET_LEN 16
#define TIME_OUT 25
#define MSG_RCVD 102
#define ID_REQUEST 103
#define SETUP_ID 104
#define SENDING 0
#define READING 1
#define COMPLETE 2
#define CONFIRM_IDSET 3

//COMMAND CODES:
#define COM 170
#define ROGER 171
#define NEGATIVE 172
#define SETID 173
#define SENDID 174
#define SETNEIGHBORS 175
#define IDSET 176
#define NEIGHBORSSET 177
#define STATE_CHANGE 178
#define AUTO 200
#define NOCOM 255

//SoftwareSerial board_port(RX_1,TX_1);

int active_channel;
int listen_count;
int state;
long prevTime;
int ledState;
uint8_t myID;
uint8_t board_ID;
uint8_t read_buf[MAX_BUFFER_SIZE];
uint8_t write_buf[MAX_BUFFER_SIZE];
int read_buf_len;
int write_buf_len;
int send_status;
int next_available_ID;
int net_map[16][5];
int net_map_len;
uint8_t con_buf[MAX_BUFFER_SIZE];
int con_buf_len;
int target_ID; //Was causing cross init errors in switch statement, had to move it.


void blink_led(int ledPin){
  long blinkTime = 250;
  long curTime = millis();
  if((curTime - prevTime) > blinkTime){
    prevTime = curTime;
    if(ledState == LOW){
      ledState = HIGH;
    }else{
      ledState = LOW;
    }
  }
  digitalWrite(ledPin, ledState);
}

void writebuffertoboards(uint8_t *buf, int buf_len){
    const uint8_t start_sig = COM;
    int temp_len = buf_len;
    uint8_t temp_buf[buf_len+1];
    while(temp_len > 0){
        temp_buf[temp_len] = buf[temp_len-1];
        temp_len--;
    }
    temp_buf[0] = start_sig;
    Serial.print("Writing");
    for(int i=0;i<buf_len+1;i++){
      Serial.print(", ");
      Serial.print(temp_buf[i], DEC);
    }
    Serial.println(" to serial the board network.");
    Serial1.write(temp_buf, buf_len+1);
}

void read_from_boards(){
  if(Serial1.read() == COM){
    int cnt=0;
    uint8_t cur_read = -1;
    while(Serial1.available() > 0 && cnt < MAX_BUFFER_SIZE && cur_read != COM){
      cur_read = Serial1.read();
      read_buf[cnt] = cur_read;
      cnt++;
    }
    read_buf_len = cnt;
    send_msg_received();
  }else{
    Serial1.flush();
  }
}

int send_msg(){
  int count = 0;
  int confirm_rcvd = 0;
  int temp_buf_len = 0;
  uint8_t temp_buf[MAX_BUFFER_SIZE];
  while(confirm_rcvd != 1 && count < TIME_OUT){
    writebuffertoboards(write_buf, write_buf_len);
    delay(30);
    if(Serial1.read() == COM){
        int cnt = 0;
        while(Serial1.available() > 0 && Serial1.peek() != COM){
            temp_buf[cnt] = Serial1.read();
            cnt++;
        }
        temp_buf_len = cnt;
        if(cnt > 0 && temp_buf[cnt-1] == ROGER){
          confirm_rcvd = 1;
          Serial.println("...");
        }else{
          while(Serial1.available() > 0)Serial1.read();
        }
    }else{
        //if the input buffer doesn't start with '170', flush the garbage.
        read_buf_len = 0;
        while(Serial1.available() > 0)Serial1.read();
    }
    count++;
  }
  delay(60);
  while(Serial1.available() > 0)Serial1.read();
  read_buf_len = 0;
  
  if(count >= TIME_OUT || temp_buf_len <= 0){
    return -1;
  }else{
    return 0;
  }
}

void send_msg_received(){
    uint8_t msg_rcvd = ROGER;
    for(int i=0;i<2;i++){
        writebuffertoboards(&msg_rcvd, 1);
        delayMicroseconds(500);
    }
    //returning with the assumption the our confirmation was received
}

int find_next_setup_ID(){
    //Set up IDs (id selection):
    int temp_row = net_map_len - 1;
    if (temp_row < 0){
        return 1;
    }else{
        //check the current row for an ID larger than the row's initial ID
        int test_ID = 0;
        while(test_ID == 0 && temp_row >= 0){
            int row_ID = net_map[temp_row][0];
            int i=0;
            int ID_found = 0;
            while(ID_found == 0 && i < 4){
                if(net_map[temp_row][i+1] < 255 && net_map[temp_row][i+1] > row_ID){
                    int temp_ID = net_map[temp_row][i+1];
                    //now check to make sure temp_ID doesn't already have an entry in the net_map
                    int entry_exists = 0;
                    for(int j=0;j<net_map_len;j++){
                        if(net_map[j][0] == temp_ID){
                            //ID already has a map entry
                            entry_exists = 1;
                        }
                    }
                    if(entry_exists == 0){
                        ID_found = 1;
                        test_ID = temp_ID;
                    }
                }
                i++;
            }
            temp_row--;
        }
        return test_ID;
    }
}

void prep_remote_msg(int target_ID){
    int path_ID = target_ID;
    write_buf_len = 0;
    if(target_ID > 1){
        Serial.print("Adding ");
        Serial.print(target_ID);
        Serial.println(" to write_buf");
        write_buf[0] = target_ID;
        write_buf_len = 1;
        //look for target ID among the ports (columns 1 thru 4) of the net_map array
        int j=0;
        int k=0;
        int done = 0;
        int found_ID = 0;
        int dir = 1;
        while(done == 0){
            for(k=0;k<4;k++){
                Serial.println(net_map[j][k+1]);
                if(net_map[j][k+1] == path_ID){
                    dir = -1;
                    found_ID = 1;
                }
            }
            j += dir;
            if(found_ID == 1){
                int index = j+1;
                //ID was found at net_map[index][0]
                path_ID = net_map[index][0];
                Serial.print("found ID: ");
                Serial.print(path_ID);
                Serial.print(" at index: ");
                Serial.println(index);
                if(path_ID > 1){
                    //shift it in to the write buffer
                    int cnt = write_buf_len;  //at this point write_buf_len is at least  '1'
                    while(cnt > 0){
                        write_buf[cnt] = write_buf[cnt-1];
                        cnt--;
                    }
                    Serial.print("Adding ");
                    Serial.print(path_ID);
                    Serial.println(" to write_buf");
                    write_buf[cnt] = path_ID;
                    write_buf_len++;
                }else{
                    done = 1;
                }
                found_ID = 0;
            }
        }
    }
    Serial.print("write_buf_len: ");
    Serial.println(write_buf_len);
    Serial.println("write_buf:");
    for(int q=0;q<write_buf_len;q++){
      Serial.print(write_buf[q]);
    }
    Serial.println("");
}

//only for use with the setneighbors command
void append_return_path(int target_ID){
    //write_buf[write_buf_len-3] should be the target ID
    int cnt = write_buf_len-4;
    while(cnt >= 0){
        write_buf[write_buf_len] = write_buf[cnt];
        cnt--;
        write_buf_len++;
    }
    if(target_ID > 1){
        write_buf[write_buf_len] = 1;
        write_buf_len++;
    }
    write_buf[write_buf_len] = 0;
    write_buf_len++;
}

void setup(){
  Serial.begin(TERMBAUD);
  Serial.println("attempting to initialize Serial comms...");
  Serial.println("...");
  Serial1.begin(TRXRATE);
  pinMode(LEDPIN, OUTPUT);
  Serial.println("...initialization complete.");
  randomSeed(analogRead(SEEDPIN));
  myID = 0;
  board_ID = -1;
  active_channel = 1;
  listen_count = 1;
  digitalWrite(LEDPIN, HIGH);
  state = SENDING;
  read_buf_len = 0;
  write_buf_len = 0;
  for(int j=0;j<16;j++){
    for(int k=0;k<5;k++){
      net_map[j][k] = -1;
    }
  }
  net_map_len = 0;
  ledState = LOW;
  next_available_ID = 1;
  prevTime = millis();
  //send_status = -1;
}

void loop(){
  switch(state){
    case SENDING:
      write_buf[0] = SENDID;
      write_buf_len = 1;
      send_status = send_msg();
      if(send_status != 0){
      //something went wrong with board to board comms
        Serial.print("Error: status code = ");
        Serial.print(send_status);
        Serial.println(". Something went wrong with board-to-board comms!!!");
      }else{
        state = READING;
      }
      break;
      
    case READING:
      delay(1);
      if(Serial1.read() == COM){
        Serial.println("COM");
        //uint8_t cur_read = -1;
        int cnt = 0;
        while(Serial1.available() > 0 && cnt < MAX_BUFFER_SIZE && (uint8_t)Serial1.peek() != COM){
          //cur_read = Serial1.read();
          read_buf[cnt] = Serial1.read(); //cur_read;
          cnt++;
          Serial.println("available");
          delay(1);
        }
        read_buf_len = cnt;
      }
      if(read_buf_len > 0){
        send_msg_received();
        Serial.println("Received message from boards:");
        for(int i=0;i<read_buf_len;i++){
          Serial.print(read_buf[i]);
        }
        Serial.println("");
        switch(read_buf[0]){
          case IDSET:
            board_ID = read_buf[1];
            next_available_ID = board_ID + 1;
            state = SETNEIGHBORS;
            break;
          case NEIGHBORSSET:
            if(read_buf_len == 7){
              for(int i=0;i<5;i++){
                net_map[net_map_len][i] = read_buf[i+1];
              }
              net_map_len++;
              next_available_ID = read_buf[6];
              state = SETNEIGHBORS;
            }else{
              //we didn't get the whole message.
              Serial.print("Read Buffer Length = ");
              Serial.println(read_buf_len);
            }
            break;
          case SENDID:
            send_status = -1;
            write_buf[0] = myID;
            write_buf_len = 1;
            send_status = send_msg();
            if(send_status != 0){
              //something went wrong with board to board comms
              Serial.print("Error: status code = ");
              Serial.print(send_status);
              Serial.println(". Something went wrong with board-to-board comms!!!");
            }else{
              state = READING;
            }
            break;
          default:
            board_ID = read_buf[0];
            state = SETID;
            break;
        }
      }
    //while(Serial1.available() > 0)Serial1.read();
    //Serial.println("%%%");
    break;
    
  case COMPLETE:
    Serial.println("Auto signal sent!");
    Serial.print("My ID is: ");
    Serial.println(myID);
    Serial.print("My Neighbor ID is: ");
    Serial.println(board_ID);
    Serial.println("System map:");
    for(int j=0;j<net_map_len;j++){
      for(int k=0;k<5;k++){
        Serial.print(net_map[j][k]);
        Serial.print(", ");
      }
      Serial.println("");
    }
    Serial.println("***** TEST COMPLETE *****");
    con_buf_len = 0;
    while(Serial.available() > 0){
      con_buf[con_buf_len] = Serial.parseInt();
      con_buf_len++;
    }
    //if there was a message from the console it will be
    //stored in con_buf
    if(con_buf_len > 0){
      if(con_buf[0] == STATE_CHANGE){
        state = con_buf[1];
      }else{
        for(int i=0;i<con_buf_len;i++){
          write_buf[i] = con_buf[i];
        }
        write_buf_len = con_buf_len;
        send_msg();
      }
    }
    delay(9000);
    break;
    
  case SETID:
    Serial.println("ID request successful!");
    Serial.print("My ID is: ");
    Serial.println(myID);
    Serial.print("My Neighbor ID is: ");
    Serial.println(board_ID);
    Serial.println("***** TEST COMPLETE *****");
    write_buf[0] = SETID;
    write_buf[1] = next_available_ID;
    write_buf_len = 2;
    send_status = send_msg();
    if(send_status != 0){
      Serial.print("Error: status code = ");
      Serial.print(send_status);
      Serial.println(". Something went wrong with board-to-board comms!!!");
    }else{
      state = READING;
    }
    break;
    
 case SETNEIGHBORS:
    target_ID = 0;
    Serial.println("Set ID successful!");
    Serial.print("My ID is: ");
    Serial.println(myID);
    Serial.print("My Neighbor ID is now: ");
    Serial.println(board_ID);
    Serial.println("***** TEST COMPLETE *****");
    target_ID = find_next_setup_ID();
    if(target_ID == 0){
        state = AUTO;
    }else{
        prep_remote_msg(target_ID);
        write_buf[write_buf_len] = SETNEIGHBORS;
        write_buf[write_buf_len+1] = next_available_ID;
        write_buf_len = write_buf_len+2;
        append_return_path(target_ID);
        send_status = send_msg();
        if(send_status != 0){
            Serial.print("Error: status code = ");
            Serial.print(send_status);
            Serial.println(". Something went wrong with board-to-board comms!!!");
        }else{
            state = READING;
        }
    }
    break;
    
 case AUTO:
 
    Serial.println("Set Neighbors Successful!");
    Serial.print("My ID is: ");
    Serial.println(myID);
    Serial.print("My Neighbor ID is: ");
    Serial.println(board_ID);
    Serial.println("System map:");
    for(int j=0;j<net_map_len;j++){
      for(int k=0;k<5;k++){
        Serial.print(net_map[j][k]);
        Serial.print(", ");
      }
      Serial.println("");
    }
    Serial.println("***** TEST COMPLETE *****");
    Serial.println("Beginning autonomous test!");
    //select the starting board from all avaiable boards in the net_map
    //start by creating an array of all known IDs
    int start_ID;
    int list_of_IDs[MAX_NET_LEN];
    int list_len;
    int rand_index;
    for(list_len=0;list_len<net_map_len;list_len++){
        list_of_IDs[list_len] = net_map[list_len][0];
    }
    //choose an index at random
    rand_index = (int)random(list_len);
    start_ID = list_of_IDs[rand_index];
    //send the AUTO command to the chosen ID
    prep_remote_msg(start_ID);
    write_buf[write_buf_len] = AUTO;
    write_buf_len++;
    Serial.print("Sending autonomous mode signal to ID = ");
    Serial.println(start_ID);
    send_msg();
    //Once the message is sent, no response from the network is currently being expected
    //this may change in future versions
    state = COMPLETE;
    break;
 default:
    break;
  }
}
